package com.sarc;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.util.Timer;
import java.util.TimerTask;

import com.sarc.games.SensorMathematics;
import com.sarc.games.focus.FocusGame;
import com.sarc.vision.PTAMSystem.NativeState;
import com.sarc.vision.PTAMSystem.OnCalibrationCompletedListener;
import com.sarc.vision.PTAMSystem.OnFrameUpdatedListener;
import com.sarc.vision.PTAMSystem.OnTrackingInitCompletedListener;
import com.sarc.vision.PTAMSystem.State;
import com.sarc.vision.PTAMSystem;

import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.core.CvType;
import org.opencv.core.Mat;

import android.opengl.GLSurfaceView;
import android.os.Bundle;
import android.os.Environment;
import android.app.Activity;
import android.graphics.PixelFormat;
import android.graphics.Point;
import android.hardware.SensorManager;
import android.util.Log;
import android.view.Display;
import android.view.Menu;
import android.view.MotionEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceHolder.Callback;
import android.view.View;
import android.view.View.OnClickListener;
import android.view.View.OnTouchListener;
import android.view.ViewGroup.LayoutParams;
import android.view.WindowManager;
import android.widget.TextView;

public class SARC extends Activity {

	
	//CameraBridgeViewBase camView;
	GLSurfaceView glView;
	String camMatFilePath;
	PTAMSystem ptam = null;
	
	/*...*/
	FocusGame focusGame = null;
	SensorMathematics sensorMath = null;
	SensorManager sensorMan = null;
	
	Timer timer;
	//boolean testGL = true;
	//boolean testPTAM = true;
	boolean useSavedData = true;
	boolean trackingInitFinished = false;
	boolean openCVLoaded = false;
	
	final String TAG = "SARC";
	
	private BaseLoaderCallback mOpenCVCallBack = new BaseLoaderCallback(this) 
	{
		@Override
		public void onManagerConnected(int status) {
		   switch (status) {
		       case LoaderCallbackInterface.SUCCESS:
		       {
	    	   		Log.i(TAG, "OpenCV loaded successfully");
					try
					{
						System.loadLibrary("nativevision");
						
						if (!openCVLoaded)
						{
				    	   	openCVLoaded = true;
							Log.d(TAG, "Custom native library loaded. Enabling Camera...");
							tryToStartGame();
							tryToStartPTAM();
							createUpdateTimer();
						}
				        
						//camView.enableView();
					}
					catch (UnsatisfiedLinkError err)
					{
						Log.d(TAG, "Could not load custom native library");
						err.printStackTrace();
					}					
					
			      //Log.d(TAG, "Library loaded");
		       } break;
		       default:
		       {
		    	   	super.onManagerConnected(status);
		       } break;
		   	}
		}
	};
	
    @Override
    public void onCreate(Bundle savedInstanceState) 
    {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);       
        
        //camView = (CameraBridgeViewBase) new JavaCameraView(this, null);
        //camView = (CameraBridgeViewBase) findViewById(R.id.mainJavaCameraView);
        getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);
        getSavedCameraDataPath();
        
        if (!OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_2_4_3, this, mOpenCVCallBack) && !openCVLoaded)
        {
          Log.e(TAG, "Cannot connect to OpenCV Manager");
        }       
	}
            
    @Override
    public void onPause()
    {
      //  if (camView != null)
        ///	camView.disableView();
        super.onPause();
    }

    @Override
    public void onResume()
    {
        super.onResume();
        if (!openCVLoaded) OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_2_4_3, this, mOpenCVCallBack);
    }
    
    @Override
    public void onDestroy()
    {
    	super.onDestroy();
    	try {
    		//timer.cancel();
			ptam.stopRunning();
			ptam.cleanup();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    	//cam.stopPreview();
    }

    @Override
    public boolean onCreateOptionsMenu(Menu menu) {
        getMenuInflater().inflate(R.menu.activity_main, menu);
        return true;
    }
    
    private void createUpdateTimer()
    {
        timer = new Timer();
        timer.schedule(new TimerTask()
        {

			@Override
			public void run() {
				final String msgString;
				State state = ptam.getCurrentState();
				if (ptam != null && ptam.isRunning())
				{
					NativeState nativeState = ptam.getNativeCurrentState();
					
					if (nativeState == NativeState.TRACKING)
					{
						Mat rv = new Mat(3, 3, CvType.CV_64F);
						Mat tv = new Mat(3, 1, CvType.CV_64F);
						ptam.getCameraExtrinsics(rv, tv);
						msgString = 
							"R:" + rv.dump() + "\n" + 
							"T:" + tv.dump() + "\n" + 
							"PTAM Tracking...";
						rv.release();
						tv.release();
					}
					else msgString = 
							"N: " + nativeState.toString() + " | " + 
							"C: " + state.toString() + " | PTAM RUNNING";
				}
				else
				{
					msgString = "C: " + state.toString() + " | PTAM NOT RUNNING";
				}
				
				final TextView infoText = (TextView)findViewById(R.id.mainInfoText);
				infoText.post(new Runnable()
				{

					@Override
					public void run() {
						// TODO Auto-generated method stub
						infoText.setText(msgString);
					}
				
				});
			}
        	
        }, 1000, 1000);
    }
    
    private void getSavedCameraDataPath()
    {
		try {
			
			String relativePath = "/Android/data/com.sarc/files/";
			String tempFilePath = Environment.getExternalStorageDirectory().getCanonicalPath();

			
			for (String s : relativePath.split("/"))
			{
				if (s != "")
				{
					tempFilePath += s;
					File f = new File(tempFilePath);
					if (!f.exists())
					{
						if (!f.mkdir())
							throw new IOException("Could not create directory: " + tempFilePath);	
					}
					tempFilePath += "/";
				}
			}

			camMatFilePath = tempFilePath + "calibration.dat";
			
			Log.d(TAG, "Calibration data path: " + camMatFilePath);
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }
    
    private void startPTAMWithSavedData()
    {
		Log.d(TAG, "TRY TO LOAD SAVED DATA FROM: " + camMatFilePath);
		try
		{
			File f = new File(camMatFilePath);
			if (f.exists())
			{
				FileInputStream fis = new FileInputStream(f);
				InputStreamReader isr = new InputStreamReader(fis);
				BufferedReader br = new BufferedReader(isr);
				Mat camMatrix = new Mat(3,3, CvType.CV_64F);
				Mat distCoef = new Mat(8,1,CvType.CV_64F);
				
				for (int i = 0; i < 3; i++)
					for (int j = 0; j < 3; j++)
					{
						String data = br.readLine();
						camMatrix.put(j, i, Double.valueOf(data).doubleValue());
					}
				
				for (int i = 0; i < 5; i++)
				{
					String data = br.readLine();
					distCoef.put(i, 0, Double.valueOf(data).doubleValue());
				}
				
				ptam.initialize(camMatrix, distCoef);
				br.close();
				isr.close();
				fis.close();
				
				Log.d(TAG, "Finished loading saved calibration data...");
				Log.d(TAG, "Camera Matrix: " + camMatrix.dump());
				Log.d(TAG, "Distortion Coefficient:" + distCoef.dump());
			}
			else
			{
				useSavedData = false;
				Log.d(TAG, "No saved file...");
			}
		}
		catch (Exception ex)
		{
			ex.printStackTrace();
			useSavedData = false;
			Log.d(TAG, "Could not use saved data...");
		}
    }
    
    private void startPTAMWithoutSavedData()
    {
		ptam.initialize();
		Log.d(TAG, "initialized without camera calibration...");
    	ptam.setOnCalibrationCompletedListener(new OnCalibrationCompletedListener()
    	{

			@Override
			public void OnCalibrationCompleted(
					Mat CameraMatrix,
					Mat DistortionCoefficients) {
				// TODO Auto-generated method stub
				Log.d(TAG, "Finished camera calibration.");
				Log.d(TAG, "Camera Matrix: " + CameraMatrix.dump());
				Log.d(TAG, "Distortion Coefficients: " + DistortionCoefficients.dump());
				File f = new File(camMatFilePath);
				
				if (!f.exists())
				{
					try {
						f.createNewFile();
					} catch (IOException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
				}
				
				try 
				{
					FileOutputStream fos = new FileOutputStream(f);
					OutputStreamWriter isw = new OutputStreamWriter(fos);
					BufferedWriter bw = new BufferedWriter(isw);
					
					for (int i = 0; i < 3; i++)
						for (int j = 0; j < 3; j++)
						{
							//CameraMatrix.
							String data = Double.valueOf(CameraMatrix.get(j, i)[0]).toString();
							bw.write(data);
							bw.newLine();
						}
					
					for (int i = 0; i < 5; i++)
					{
						String data = Double.valueOf(DistortionCoefficients.get(i, 0)[0]).toString();
						bw.write(data);
						bw.newLine();
					}
					
					bw.close();
					isw.close();
					fos.close();
					
				} catch (FileNotFoundException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
			}
    		
    	});
    }
    
    private void tryToStartPTAM()
    {
    	ptam = new PTAMSystem(true, true);
		
		if (useSavedData)
		{
			startPTAMWithSavedData();
		}
			
		if (!useSavedData)
		{
			startPTAMWithoutSavedData();
		}
			
		ptam.start();
		Log.d(TAG, "PTAM Starting...");
		
		/*
        camView.setMaxFrameSize(640, 480);
        camView.setOnClickListener(new OnClickListener()
        {

			@Override
			public void onClick(View arg0) {
				// TODO Auto-generated method stub
				ptam.captureOnNextFrame();
			}
    	   
       	});
        camView.setOnLongClickListener(new OnLongClickListener()
        {

			@Override
			public boolean onLongClick(View arg0) {
				// TODO Auto-generated method stub
				ptam.resetOnNextFrame();
				return true;
			}
        	
        });
        
        
        camView.setCvCameraViewListener(new CvCameraViewListener()
        {

			@Override
			public void onCameraViewStarted(int width,
					int height) {
				// TODO Auto-generated method stub
				ptam.resumeRunning();
			}

			@Override
			public void onCameraViewStopped() {
				// TODO Auto-generated method stub
				ptam.pauseRunning();
			}

			@Override
			public Mat onCameraFrame(Mat inputFrame) {
				// TODO Auto-generated method stub
				Log.d(TAG,"cameraframe1");
				ptam.pushNewFrame(inputFrame);
				ptam.retrieveFromOutputFrame(inputFrame);
				if (focusGame != null)
				{
					focusGame.setNewImageBuffer(inputFrame);
					Log.d(TAG,"cameraframe2");
				}
				return inputFrame;
			}
        	
        });
        */
		
		ptam.setOnTrackingInitCompletedListener(new OnTrackingInitCompletedListener()
		{

			@Override
			public void OnTrackingInitCompleted(Mat rv, Mat tv, Mat u, Mat v) 
			{
				// TODO Auto-generated method stub
				
				Mat refPtMat = new Mat(1,3, CvType.CV_32FC1);
				ptam.getReferencePoint(refPtMat);
				trackingInitFinished = true;
				
				focusGame.setReferencePoint(refPtMat);
				focusGame.setNewCameraPosition(rv, tv);
				//focusGame.setNormal(u,v);
				focusGame.setShouldDrawScene(true);
				Log.d(TAG, "tracking init completed");
				refPtMat.release();
			}
		});

		ptam.setOnFrameUpdatedListener(new OnFrameUpdatedListener()
		{
			@Override
			public void OnFrameUpdated(Mat frame, NativeState ptamState) {
				// TODO Auto-generated method stub
				if (focusGame != null)
				{
					focusGame.setNewImageBuffer(frame);
					if (ptamState == NativeState.TRACKING)
					{
						Mat rv = new Mat(3,1,CvType.CV_32FC1);
						Mat tv = new Mat(3,1,CvType.CV_32FC1);
						if (ptam.getCameraExtrinsics(rv, tv))
						{
							Log.d(TAG, "succ" + tv.dump() + rv.dump());
						}
						
						focusGame.setNewCameraPosition(rv, tv);
						rv.release();
						tv.release();
					}
				}
			}
			
		});
		
		glView.setOnTouchListener(new OnTouchListener()
		{

			@Override
			public boolean onTouch(View arg0, MotionEvent arg1) {
				// TODO Auto-generated method stub
				boolean consumed = false;
				if (trackingInitFinished)
				{
					if (focusGame != null)
					{
						consumed = focusGame.onUserTouch(arg1);
					}
				}
				else
				{
					if (arg1.getAction() == MotionEvent.ACTION_DOWN)
					{
						ptam.captureOnNextFrame();	
					}
				}
				return consumed;
			}
			
		});
	
    }
    
    private void tryToStartGame()
    {
    	//glView = (GLSurfaceView) findViewById(R.id.mainGLsurfaceView);
		glView = new GLSurfaceView(this);
    	//glView = new GLSurfaceView(this);
		sensorMan = (SensorManager) getSystemService(SENSOR_SERVICE);
		sensorMath = new SensorMathematics(sensorMan);
		sensorMath.RecorderStart();
		
		Display display = getWindowManager().getDefaultDisplay();
		Point winSize = new Point();
		display.getSize(winSize);
		focusGame = new FocusGame(glView, sensorMath, (float)winSize.x, (float)winSize.y);
		glView.setEGLConfigChooser(8,8,8,8,0,0);
		glView.setRenderer(focusGame);
    	glView.setLayoutParams(new LayoutParams	(LayoutParams.MATCH_PARENT, LayoutParams.MATCH_PARENT));
    	//glView.setEGLConfigChooser(8,8,8,8,16,0);
    	//glView.setZOrderOnTop(true);
   
    	glView.getHolder().addCallback(new Callback()
    	{

			@Override
			public void surfaceChanged(SurfaceHolder holder, int format,
					int width, int height) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void surfaceCreated(SurfaceHolder holder) {
		    	holder.setFormat(PixelFormat.TRANSLUCENT);
			}

			@Override
			public void surfaceDestroyed(SurfaceHolder holder) {
				// TODO Auto-generated method stub
				
			}
    		
    	});
    	this.addContentView(glView, new LayoutParams(LayoutParams.MATCH_PARENT, LayoutParams.MATCH_PARENT));
	}
    
}
